#include "graph.h"
#include "connection.h"
#include "node.h"
#include "priorityheap.h"
#include "astarheuristics.h"
#include "astarsearch.h"

AStarSearch::AStarSearch(Graph* graph, uint32 start, uint32 goal)
: graph_(graph),
startNode_(start),
goalNode_(goal),
shortestPath_(graph->getNodeAmount()),
nodeRecords_(graph->getNodeAmount()),
visitedConnections_(0),
iterations_(0)
{
	openList_ = new PriorityHeap<NodeRecord>(graph->getNodeAmount());
}

AStarSearch::~AStarSearch()
{

}

void AStarSearch::search()
{
	NodeRecord startRecord;
	startRecord.setNode(startNode_);
	startRecord.setCostSoFar(1.0);
	openList_->insert(startRecord);

	Node* goal = graph_->getNode(goalNode_);
	Ogre::Vector2 goalPosition = goal->getPosition();

	while(!openList_->isEmpty())
	{
		++iterations_;
		NodeRecord currentRecord = openList_->findMin();
		openList_->deleteMin();


		//add connection to shortest path
		shortestPath_[currentRecord.getNode()] = currentRecord.getConnection();

		//goal found -> stop searching
		if(currentRecord.getNode() == goalNode_)
		{
			std::cout << "Found goal" << std::endl;
			return;
		}

		//get connection of current node
		Graph::AdjacentConnections nodeConnections = graph_->getConnections(currentRecord.getNode());
		Graph::AdjacentConnections::const_iterator connectionIt;

		for(connectionIt = nodeConnections.begin();connectionIt!=nodeConnections.end();++connectionIt)
		{
			Connection* currentConnection = (*connectionIt);
			std::cout << "Exploring connection between node " << currentConnection->getFromNode() << " and node " << currentConnection->getToNode() << std::endl;
			++visitedConnections_;

			//calculate heuristic costs
			Ogre::Vector2 toNodePosition = graph_->getNode(currentConnection->getToNode())->getPosition();
			float heuristicCosts = getEuclideanDistance(toNodePosition.x, toNodePosition.y, goalPosition.x, goalPosition.y);

			//calculate real costs
			float costSoFar = currentRecord.getCostSoFar() + currentConnection->getCost();

			//process unvisited nodes, if there is no NodeRecord instance, the node has not been processed yet
			if(nodeRecords_[currentConnection->getToNode()] == NULL)
			{
				NodeRecord* newRecord = new NodeRecord;
				newRecord->setNode(currentConnection->getToNode());
				newRecord->setCostSoFar(costSoFar);
				newRecord->setEstimatedCost(costSoFar + heuristicCosts);
				newRecord->setConnection(currentConnection);
				openList_->insert(*newRecord);

				nodeRecords_[currentConnection->getToNode()] = newRecord;
				
			}
			//process nodes that have already been visited and check if an alternative route is cheaper 
			else if(nodeRecords_[currentConnection->getToNode()]->getCostSoFar() > costSoFar
				&& shortestPath_[currentConnection->getToNode() == NULL])
			{
				currentRecord.setCostSoFar(costSoFar);
				currentRecord.setEstimatedCost(costSoFar + heuristicCosts);
			}
		}
	}
}

//std::vector<const Connection*> AStarSearch::getShortestPath()
//{
//	return shortestPath_;
//}

std::list<uint32> AStarSearch::getShortestPath()
{
	std::list<uint32> path;

	if(shortestPath_[goalNode_] == NULL)
	{
		std::cerr << "No path found "<< std::endl;
		return path;
	}
	else 
	{
		uint32 currentNode = goalNode_;
		
		//assemble path by going backwards from the goal node
		path.push_front(goalNode_);
		while(currentNode != startNode_ && shortestPath_[currentNode] != NULL)
		{
			Connection* currentConnection = shortestPath_[currentNode];
			currentNode = currentConnection->getFromNode();
			path.push_front(currentNode);
		}
	}
	return path;
}

void AStarSearch::printStatistics()
{

	uint32 graphFill = 0;

	for(size_t i = 0; i< nodeRecords_.size();++i)
	{
		if(nodeRecords_[i] != NULL)
		{
			++graphFill;
		}
	}

	std::cout << "Explored connections: " << visitedConnections_ << std::endl;
	std::cout << "Visited nodes: " << graphFill << std::endl;
	std::cout << "A* Iterations: " << iterations_ << std::endl;
}